package pathAlgorithms;

import java.util.LinkedList;
import javax.vecmath.Point2d;

public class RoboPath implements Comparable<RoboPath> {
	
	
	public static int id = 0;
	
	private RoboPath path;
	private RoboNode node;
	private double pathLength;
	private double distanceToTarget;
	private int pathId;

	/**
	 * 
	 * @param path to node
	 * @param node last visited
	 * @param distanceToTarget
	 */
	public RoboPath(RoboPath path, RoboNode node, double distanceToTarget) {
		super();
		this.path = path;
		this.node = node;

		this.distanceToTarget = distanceToTarget;
		
		if (path != null) {
			this.pathLength = node.getPosition().distance(path.getLast().getPosition())
				+ path.getPathLength();
		} else {
			this.pathLength = 0;
		}
		
		this.pathId = id++;
	}
		
	public int getPathId() {
		return pathId;
	}


	public double getDistanceToTarget() {
		return distanceToTarget;
	}


	public double getPathLength() {
		return pathLength;
	}
		
	public Point2d[] getPositions() {
		LinkedList<Point2d> list = new LinkedList<Point2d>();
		RoboPath it = path;
		
		while (it != null) {
			list.addFirst(it.getLast().getPosition());
			it = it.path;
		}
		list.removeFirst();
		list.addLast(node.getPosition());
		return list.toArray(new Point2d[0]);
	}
	
	
	public RoboNode getLast() {
		return node;
	}

	@Override
	public int compareTo(RoboPath o) {		
		return Double.compare(
				getPathLength() + getDistanceToTarget()
				, o.getPathLength() + o.getDistanceToTarget());
	}
	
	@Override
	public boolean equals(Object obj) {
		RoboPath otherPath;
		if(obj instanceof RoboPath) {
			otherPath = (RoboPath)obj;
		}else {
			return false;
		}
		if(path == null) {
			return node.equals(otherPath.node);
		}else {
			return this.path.equals(otherPath.path) && this.node.equals(otherPath.node);
		}
		
	}
	
}
